#include "troyglview.h"

TroyGLView::TroyGLView(QWidget *parent) : OpenGLDisplay(parent)
{
    _selected = NULL;
    setView(THREE_QUARTER);
}


void TroyGLView::resizeGL(int width, int height){
    //Set the perspective/view
    glViewport(0, 0, width, height);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    gluPerspective(70.0, ((float)width / (float)height), 1.0, 40.0);
    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();

    if(_curView == THREE_QUARTER){
        gluLookAt(5.0, 11.0, 20.0, 0.0, 10.0, 0.0, 0.0, 1.0, 0.0);
    }
    if(_curView == FRONT){
        gluLookAt(0.0, 15.0, 20.0, 0.0, 10.0, 0.0, 0.0, 1.0, 0.0);
    }
}


void TroyGLView::drawScene(){
    //qDebug() << "Draw scene";
    if(_model != NULL)
        drawRobot();
}

void TroyGLView::drawRobot(){
    //qDebug() << "Draw robot";

    //Initialize the name stack for selection.
    //Ignored by OpenGL if not in selection mode.
    glInitNames();

    //Create a ComponentGLDrawer that will visit the
    //components of the robot and draw them
    ComponentGLDrawer drawer;
    _model->accept(&drawer);

    glFlush();
}


TroyGLView::ComponentGLDrawer::ComponentGLDrawer(){

}


void TroyGLView::ComponentGLDrawer::pre(){
    glPushMatrix();
}

void TroyGLView::ComponentGLDrawer::post(){
    glPopMatrix();
}


void TroyGLView::ComponentGLDrawer::visit(RobotComponent* component){
    //Used for calculations
    QVector3D offset;
    Dimension3D dimensions;
    QQuaternion rotation;
    QVector3D rotationAxis;
    float rotationAngle, halfLengthTranslation;

    glPushName(component->name());

    //Translate to the offset
    offset = component->offset();
    glTranslatef(offset.x(), offset.y(), offset.z());

    dimensions = component->dimensions();
    //Translate to the center of the component along its length.
    //It is assumed that the "length" of the component is along the -y axis
    //unless component.inverted() is TRUE, in which case the length is +y
    halfLengthTranslation = (dimensions.y / 2);
    if(!component->inverted())
        halfLengthTranslation *= -1;
    glTranslatef(0.0, halfLengthTranslation, 0.0);

    //Draw the component
    if(component->type() == JOINT_GROUP){
        OpenGLDisplay::wireSphere(dimensions.x / 2, 10, 8);
        //Rotate at this point
        rotation = ((RobotJointGroup*)(component))->rotation();
        //Convert from quaternion to axis/angle
        rotationAxis = rotation.vector();
        rotationAngle = RADIANS_TO_DEGREES(acos(rotation.scalar()) * 2.0f);
        glRotatef(rotationAngle, rotationAxis.x(), rotationAxis.y(), rotationAxis.z());
    }
    else if(component->type() == SEGMENT){
        glPushMatrix();
        glScalef(dimensions.x, dimensions.y, dimensions.z);
        OpenGLDisplay::wireCube(1.0);
        glPopMatrix();
    }

    //We rendered everything at the center of the component,
    //translate to the end
    glTranslatef(0.0, halfLengthTranslation, 0.0);
    glPopName();
}



void TroyGLView::setView(ViewOption option){
    _curView = option;
}


void TroyGLView::setSelectionColor(){
    glColor3f(_selected->selectionColor[0],
              _selected->selectionColor[1],
              _selected->selectionColor[2]);
}

void TroyGLView::selectComponent(RobotGLController::GUIComponent* component){
    _selected = component;
    updateGL();
}

void TroyGLView::deselectComponent(RobotGLController::GUIComponent *component){
    _selected = NULL;
    updateGL();
}

